#include <sensor_msgs/msg/point_field.hpp>
#include <tf2/buffer_core.h>
#include <lidar209_msgs/msg/lidar209_scan_ch88.hpp>

#include <memory>
#include <string>

#include "pointcloudXYZIR.hpp"

namespace lidar209_pointcloud
{

PointcloudXYZIR::PointcloudXYZIR(
  const double min_range, const double max_range,
  const std::string & target_frame, const std::string & fixed_frame,
  const unsigned int scans_per_block, tf2::BufferCore & tf_buffer)
: DataContainerBase(
    min_range, max_range, target_frame, fixed_frame,
    0, 1, true, scans_per_block, tf_buffer, 5,
    "x", 1, sensor_msgs::msg::PointField::FLOAT32,
    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
    "z", 1, sensor_msgs::msg::PointField::FLOAT32,
    "intensity", 1, sensor_msgs::msg::PointField::FLOAT32,
    "ring", 1, sensor_msgs::msg::PointField::UINT16),
  iter_x_(cloud, "x"), iter_y_(cloud, "y"), iter_z_(cloud, "z"),
  iter_intensity_(cloud, "intensity"), iter_ring_(cloud, "ring")
{}

void PointcloudXYZIR::setup(const lidar209_msgs::msg::Lidar209ScanCH88::SharedPtr scan_msg)
{
  DataContainerBase::setup(scan_msg);
  iter_x_ = sensor_msgs::PointCloud2Iterator<float>(cloud, "x");
  iter_y_ = sensor_msgs::PointCloud2Iterator<float>(cloud, "y");
  iter_z_ = sensor_msgs::PointCloud2Iterator<float>(cloud, "z");
  iter_intensity_ = sensor_msgs::PointCloud2Iterator<float>(cloud, "intensity");
  iter_ring_ = sensor_msgs::PointCloud2Iterator<uint16_t>(cloud, "ring");
}

void PointcloudXYZIR::newLine()
{
}

void PointcloudXYZIR::addPoint(
  float x, float y, float z, uint16_t ring,
  float distance, float intensity)
{
  if (!pointInRange(distance)) {
    return;
  }

  // convert polar coordinates to Euclidean XYZ

  if (config_.transform) {
    transformPoint(x, y, z);
  }

  *iter_x_ = x;
  *iter_y_ = y;
  *iter_z_ = z;
  *iter_ring_ = ring;
  *iter_intensity_ = intensity;

  ++cloud.width;
  ++iter_x_;
  ++iter_y_;
  ++iter_z_;
  ++iter_ring_;
  ++iter_intensity_;
}

}  // namespace lidar209_pointcloud
